#include <lcm/lcm.h>
#include <bot_core/bot_core.h>
#include <lcmtypes/quad_lcmtypes.h>
#include <quad_controller/controller_interface.h>
#include <bot_param/param_client.h>

// NOTE: uses rc_rawdata RAW values, not calibrated
// for some reason calib throttle/yaw = 0 when the serial switch is on

struct State {
  lcm_t* lcm;
  const char* publish_channel;
};

// values configured with Spektrum
static int32_t MAX_STICK_VAL = 14310;
static int32_t MIN_STICK_VAL = 8113;
static int32_t DEAD_ZONE_RAD = 10;
static int32_t DEAD_ZONE_RAD_Z = 500;
static double MAX_VEL_CMD = 1.0;  // m/s
static double MAX_YAW_VEL_CMD = 1.0;  // rad/s
static double MAX_Z_VEL_CMD = 0.5;

double rc_rawdata_poll_rate;  // get from param server

static void rc_data_handler(const lcm_recv_buf_t* rbuf, const char* channel,
                            const carmen3d_quad_rc_data_msg_t* msg,
                            void* user) {

  State* state = (State*) user;

  fprintf(stderr, "RC: lock: %c raw[0]: %d [1]: %d [2]: %d [3]: %d [4]: %d\n",
          msg->lock, msg->raw_values[0], msg->raw_values[1], msg->raw_values[2],
          msg->raw_values[3], msg->raw_values[4]);

  // ensure rc lock
  if (!msg->lock) {
    fprintf(stderr, "ignoring: no rc lock\n");
    return;
  }

  // ensure serial mode is enabled
  if (msg->raw_values[4] != MAX_STICK_VAL) {
    fprintf(stderr, "ignoring: serial mode is not enabled\n");
    return;
  }

  // compile normalized stick positions (-1:1)
  int32_t stick_center = MIN_STICK_VAL
      + int(float(MAX_STICK_VAL - MIN_STICK_VAL) / 2.0);
  double stick[4];
  double stick_span = MAX_STICK_VAL - MIN_STICK_VAL;

  double vel_span = 2.0 * MAX_VEL_CMD;

  // normalize stick velocity
  stick[1] = vel_span * double(msg->raw_values[1] - MIN_STICK_VAL) / stick_span
      + -1.0 * MAX_VEL_CMD;
  stick[0] = vel_span * double(msg->raw_values[0] - MIN_STICK_VAL) / stick_span
      + -1.0 * MAX_VEL_CMD;
  stick[2] = 2.0 * MAX_Z_VEL_CMD * double(msg->raw_values[2] - MIN_STICK_VAL)
      / stick_span + -1.0 * MAX_Z_VEL_CMD;
  stick[3] = 2.0 * MAX_YAW_VEL_CMD * double(msg->raw_values[3] - MIN_STICK_VAL)
      / stick_span + -1.0 * MAX_YAW_VEL_CMD;

  // overwrite with dead zone
  for (int i = 0; i < 4; i++) {
    // strickter z deadzone
    if (i == 2) {
      if (msg->raw_values[i] > stick_center - DEAD_ZONE_RAD_Z
          && msg->raw_values[i] < stick_center + DEAD_ZONE_RAD_Z) {
        // stick in dead zone
        stick[i] = 0.0;
      }
    } else {
      // normal dead zone
      if (msg->raw_values[i] > stick_center - DEAD_ZONE_RAD
          && msg->raw_values[i] < stick_center + DEAD_ZONE_RAD) {
        // stick in dead zone
        stick[i] = 0.0;
      }
    }

  }
  fprintf(stderr, "sx=%f sy=%f sz=%f syaw=%f", stick[1], stick[0], stick[2],
          stick[3]);

  quad_waypoint_t waypt_message;
  carmen3d_controller_set_noop(&waypt_message);  // initialize values

  // relative waypoint vector normalized to poll rate (e.g. 10 Hz -> 10 cm)
  /*
   double xyz_vec_length = sqrt( stick[1]*stick[1] + stick[0]*stick[0] + stick[2]*stick[2] );
   if (xyz_vec_length == 0) {
   xyz_vec_length = 1; // no nan in calc's below
   }
   */

  /*
   waypt_message.xyzt[0] = stick[1] / xyz_vec_length * rc_rawdata_poll_rate;
   waypt_message.xyzt[1] = stick[0] / xyz_vec_length * rc_rawdata_poll_rate;
   waypt_message.xyzt[2] = stick[2] / xyz_vec_length * rc_rawdata_poll_rate;
   waypt_message.xyzt[3] = stick[3] * rc_rawdata_poll_rate; // TBD ensure M_PI?
   waypt_message.xyzt_dot[0] = stick[1];
   waypt_message.xyzt_dot[1] = stick[0];
   waypt_message.xyzt_dot[2] = stick[2];
   waypt_message.xyzt_dot[3] = stick[3];

   waypt_message.waypt_type = QUAD_WAYPOINT_T_TYPE_BODY_RELATIVE; //QUAD_WAYPOINT_T_TYPE_WAYPT;
   waypt_message.nonce = random();
   waypt_message.utime = bot_timestamp_now();
   waypt_message.sender = QUAD_WAYPOINT_T_SENDER_WAYPOINT_TOOL;
   carmen3d_controller_sanity_check_and_publish(state->lcm, state->publish_channel, &waypt_message);

   fprintf(stderr, "SENDING: X=%+4.2f Y=%+4.2f Z=%+4.2f yaw=%+4.2f vX=%+4.2f vY=%+4.2f vZ=%+4.2f type=%d\n", waypt_message.xyzt[0],
   waypt_message.xyzt[1], waypt_message.xyzt[2], bot_to_degrees(waypt_message.xyzt[3]), waypt_message.xyzt_dot[0], waypt_message.xyzt_dot[1],
   waypt_message.xyzt_dot[2], waypt_message.waypt_type);
   */

  if (stick[2] != 0.0 || stick[3] != 0.0) {
    carmen3d_controller_publish_relative_waypoint(
        state->lcm, 0.0, 0.0, stick[2] * rc_rawdata_poll_rate,
        stick[3] * rc_rawdata_poll_rate, bot_timestamp_now());
  }

  if (stick[1] != 0.0 || stick[0] != 0.0) {
    carmen3d_controller_publish_body_relative_waypoint(
        state->lcm, stick[1] * rc_rawdata_poll_rate,
        stick[0] * rc_rawdata_poll_rate, bot_timestamp_now());
  }

  fprintf(stderr, "X=%.3f Y=%.3f Z=%.3f Yaw=%.3f\n",
          stick[1] * rc_rawdata_poll_rate, stick[0] * rc_rawdata_poll_rate,
          stick[2] * rc_rawdata_poll_rate,
          bot_to_degrees(stick[3] * rc_rawdata_poll_rate));

}

int main(int argc, char** argv) {

  State* state = new State();

  printf("RC-Joystick\n");
  printf(
      "  when serial mode enabled, RC transmitter sends relative waypoint commands\n");

  state->lcm = bot_lcm_get_global(NULL);
  state->publish_channel = WAYPOINT_COMMAND_CHANNEL;

  BotParam* config = bot_param_new_from_server(state->lcm, 0);
  rc_rawdata_poll_rate = bot_param_get_double_or_fail(
      config, "quadcomm.subscriptions.rc_rawdata");
  fprintf(stderr, "RC DATA POLL RATE: %f\n", rc_rawdata_poll_rate);
  bot_param_destroy(config);

  carmen3d_quad_rc_data_msg_t_subscribe(state->lcm, ASCTEC_RC_DATA_CHANNEL,
                                        rc_data_handler, state);

  while (1) {
    lcm_handle(state->lcm);
  }

  lcm_destroy(state->lcm);
  delete state;

  return 0;
}

